Cubicle Merge

Cubicle merge package handles the short-term obstacle mapping function, while in contrast, SSLAM Localization handles sparse feature map for long-term localization, and the cubicle merge package tries to manage and display obstacle map on ROS RVIZ. To keep a clear trajectory of vehicle, we stop displaying them after they are invisible.

This package has 4 modules:

1. Data Acquisition

• Subscribe the realtime SSLAM Localization SSLAM Localization output

  Pose from current camera frame to world coordinate (/tf)

  Set the pose from frame \(k\) to world coordinate: translation \(\mathbf{t}_k^w\) and rotation \(\mathbf{R}_k^w\), respectively; \(k\) is the index of the frame.

• Subscribe the realtime Cubicle Detect Cubicle Detection output

  The object information (size, position, etc.) w.r.t. the current camera frame (/obstacle_msgs).

  Set the received local obstacle position \(\mathbf{P}_i^k\), w.r.t. the \(k\) th frame, where \(i\) is the index of obstacle.

2. Conversion From Local to World Coordinate

Based on the received data, we can convert the \(i\) th the obstacle pose under the \(k\) th frame into world coordinate

\[\begin{equation} \mathbf{P}_i^w = \mathbf{R}_k^w \cdot \mathbf{P}_i^k + \mathbf{t}_k^w \end{equation}\]

3. Obstacle Merging

After conversion, we save the obstacle into our obstacle database mapDatabase. The data structure of our database is std::map<index_t, MapElement *> where MapElement refers to Class MapElement, which defines all the properties and behaviors of an object.

The advantage of std::map is that it is a sorted associative container that contains key-value paris with unique keys. So each time we insert a new coming message, we don’t need to worry about dupicates of the same obstacles. We can easily compare and update existing obstacles, making the whole obstacle mapping process consistent and neat.

As proper tracking has already been done in Cubicle Detection, we don’t adopt complicated logics to merge new obstacles. The strategy is as follows:

• For each frame

  We add lost_frame by 1.

• New coming obstacle with new ID that mapDatabase does not container

  • Insert the obstacle to mapDatabase.

  • Set tracking age as 1.

• New coming obstacle with ID that already exists in mapDatabase

  • Compare the new global position with last observed position, calculate the velocity between two observations.

  • If confirmed valid, overwrite the old observation with the new one.

  • tracking age \(+1\).

• For all new observations

  We reduce lost_frame by 1.

So we dispose of all the obstacle observations that have lost_frame smaller than a threshold. For better smoothness, we also disregard observations that have tracking age smaller than another threshold.

4. Visualization

With the above 3 modules, the process of visualization is simple. We display all the obstacles in the format of visualization_msgs::Marker with different properties.

• For designated classes (like vehicles and pedestrians)

  We provide MESH_RESOURCE for different types. Thus you can see cars and persons on the RVIZ map.

• For obstacles with unkown semantic class information

  We render them red CUBE with the observed width and height.

Fig. 2 Illustration of cubicle merge performance, the colorful points are laser points.